#include<Algorithm/qem.h>
#include<Mesh/libcell_iterator.h>
#include<tools_vector.h>
#include<Mesh/_Func_.h>
#define quote lib_cell_quote


static LB_Matrix* matrix_add(LB_Matrix* m1,LB_Matrix* m2)
{
    LB_Matrix* re=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(re,4,4);

    double* data=(double*)(re->data),*data1=(double*)(m1->data),*data2=(double*)(m2->data);
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            data[i*4+j]=data1[i*4+j]+data2[i*4+j];
        }
    }
    return re;
}

static LB_Matrix* get_matrix_q_from_cell(template_c* c)
{
    LB_Matrix* re=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(re,4,4);

    double  plane[4]={0};
    if(get_plane_equation_of_triangular_plane(c->vertices[0]->point,
        c->vertices[1]->point,c->vertices[2]->point,plane)==0)
    {
        lb_matrix_free(re);
        return NULL;
    }


    double* data=(double*)(re->data);
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            data[i*4+j]=plane[i]*plane[j];
        }
    }
    // free(plane);
    return re;
}


static Int_RB_Tree* compute_matrix_q_every_cell(Mesh* m)
{
    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        LB_Matrix* mat=get_matrix_q_from_cell(quote(cit));
        if(mat==NULL)
        {
            continue;
        }
        re->insert(re,quote(cit)->id,mat);
    }

    return re;
}


static LB_Matrix * get_matrix_q_from_vertex(template_v* v,Int_RB_Tree* tree)
{
    LB_Matrix* re=(LB_Matrix*)malloc(sizeof(LB_Matrix));
    lb_matrix_init_double(re,4,4);
    re->zero(re);
    double* data=(double*)(re->data);
    // for(Node* nit=v->cells;nit!=NULL;nit=(Node*)(nit->Next))
    for(int k=0;k<v->cellsn.size;k++)
    {
        template_c* c=(template_c*)(v->cellsn.extra_data[k]);
        // template_c* c=(template_c*)(nit->value);
        //Matrix4x4* m=get_matrix_q_from_cell(c);
        LB_Matrix* m=(LB_Matrix*)tree->find(tree,c->id);

        if(m==NULL)
        {
            continue;
        }
        double* data1=(double*)(m->data);
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                data[i*4+j]+=data1[i*4+j];
            }
        }
        //Matrix4x4_free(m);
    }

    return re;
}



Int_RB_Tree* qem_init_qi(Mesh * m)
{
    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    Int_RB_Tree* tree= compute_matrix_q_every_cell(m);

    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        LB_Matrix* m= get_matrix_q_from_vertex(quote(vit),tree);
        re->insert(re,quote(vit)->id,m);
    }
    for(auto it=tree->begin(tree);it.it!=NULL;it++)
    {
        lb_matrix_free((LB_Matrix*)(it.second));
    }
    int_rb_tree_free(tree);
    return re;
}


static inline double*  compute_qem_one_edge_position(Mesh* m,template_f* f,Int_RB_Tree *tree)
{
    double* p=NULL;
    template_v* v1=f->vertices[0],*v2=f->vertices[1];
    LB_Matrix* m1=(LB_Matrix*)(tree->find(tree,v1->id));
    LB_Matrix* m2=(LB_Matrix*)(tree->find(tree,v2->id));
    LB_Matrix* m3=matrix_add(m1,m2);
    double* data=(double*)(m3->data);
    data[12]=0;data[13]=0;data[14]=0;data[15]=1;
    LB_Matrix* mi=m3->inverse(m3);
    double mid_p[3]={(v1->point[0]+v2->point[0])/2.0,(v1->point[1]+v2->point[1])/2.0,(v1->point[2]+v2->point[2])/2.0};

    if(mi!=NULL)
    {
        double* datami=(double*)(mi->data);
        p=(double*)malloc(sizeof(double)*3);
        memset(p,0,sizeof(double)*3);
        p[0]=datami[0*4+3];p[1]=datami[1*4+3];p[2]=datami[2*4+3];
        if(distance_of_two_points(mid_p,p,3)>2*distance_of_two_points(mid_p,v1->point,3))
        {
            p[0]=mid_p[0];p[1]=mid_p[1];p[2]=mid_p[2];
        }
        lb_matrix_free(mi);
    }
    else
    {
        p= (double*)malloc(sizeof(double)*3);
        memset(p,0,sizeof(double)*3);
        //p[0]=mid_p[0];p[1]=mid_p[1];p[2]=mid_p[2];
        p[0]=v1->point[0];p[1]=v1->point[1];p[2]=v1->point[2];

    }
    lb_matrix_free(m3);
    return p;

}

static double compute_xTAx(LB_Matrix* Q,double* p)
{
    double* data=(double*)(Q->data);
    double p1[4]={0,0,0,0},p2[4]={p[0],p[1],p[2],1};
    for(int i=0;i<4;i++)
    {
        double temp=0;
        for(int j=0;j<4;j++)
        {
            temp+=data[i*4+j]*p2[j];
        }
        p1[i]=temp;
    }
    return (p2[0]*p1[0]+p2[1]*p1[1]+p2[2]*p1[2]+p2[3]*p1[3]);
}
//
// tree是每个边对应的点坐标
// tree1是每个点对应的q矩阵
//
static inline double qem_compute_one_edge_cost(template_f*f ,Int_RB_Tree*tree,double* p)
{
    if(p==NULL)
    {
        return -1;
    }
    LB_Matrix* m1=(LB_Matrix*)(tree->find(tree,f->vertices[0]->id));
    LB_Matrix* m2=(LB_Matrix*)(tree->find(tree,f->vertices[1]->id));

    LB_Matrix *m3=matrix_add(m1,m2);
    double re= compute_xTAx(m3,p);
    lb_matrix_free(m3);

    return re;
}


static int edge_energy_compare(const void* v1,const void* v2)
{
    Edge_Energy*ee1=((Edge_Energy*)(v1)),*ee2=((Edge_Energy*)(v2));
    return ee1->energy>ee2->energy?1:(ee2->energy>ee1->energy?(-1):0);
}


static inline int is_edge_legacy_to_collapse(Mesh* m,template_f* f)
{
    template_v* v1=f->vertices[0],*v2=f->vertices[1];
    LB_Vector_voidptr vec1=Mesh_vv_beginn(m,*v1);
    LB_Vector_voidptr vec2=Mesh_vv_beginn(m,*v2);
    int size=0;
    for(int i=0;i<vec1.size;i++)
    {
        void* value=*(vec1.at(&vec1,i));
        for(int j=0;j<vec2.size;j++)
        {
            if(*(vec2.at(&vec2,j))==value)
            {
                size++;break;
            }
        }
    }

    vec1.clear(&vec1);vec2.clear(&vec2);
    return size;
    // Node*n1=m->vv_begin(m,*v1),*n2=m->vv_begin(m,*v2);
    // Node* n=node_intersection(n1,n2);
    // int size=node_size(n);
    // free_node(n);free_node(n1);free_node(n2);
    // return size;
}

// collapse需要修改判断 ，此外 qem_collapse_edge也存在折叠情况
//
CHeap *compute_qem_all_edge_position_and_energy(Mesh* m,Int_RB_Tree*tree)
{
    CHeap *ch=(CHeap*)malloc(sizeof(CHeap));
    cheap_init(ch);
    Edge_Energy** values=(Edge_Energy**)malloc(sizeof(Edge_Energy*)*(m->num_f(m)+15000));
    memset(values,0,sizeof(Edge_Energy*)*(m->num_f(m)+15000));ch->max_size=m->num_f(m)+15000;
    ch->values=(void**)values;
    ch->size=0;
    //ch->width=sizeof(void*);
    ch->comp=edge_energy_compare;

    for(auto fit=m->f_begin(m);fit!=m->f_end(m);fit++)
    {
        int size=is_edge_legacy_to_collapse(m,quote(fit));
        if(size==2)
        {
            double* p=compute_qem_one_edge_position(m,quote(fit),tree);
            if(p==NULL)
            {
                continue;
            }
            double energy=qem_compute_one_edge_cost(quote(fit),tree,p);

            Edge_Energy* data=(Edge_Energy*)malloc(sizeof(Edge_Energy));
            edge_energy_init(data);
            data->energy=energy;
            data->p=p;
            data->fid=quote(fit)->id;
            values[ch->size]=data;
            ch->size=ch->size+1;
        }
        else if(size>2)
        {
        }
        else
        {
        }
    }
    printf("begin make cheap\n");
    make_cheap(ch);
    printf("end mak cheap\n");
    return ch;
}

template_v* qem_collapse_edge(Mesh *m ,Edge_Energy*ee,
    Int_RB_Tree* tree_v,CHeap*ch)
{
    if(ee==NULL||ee->p==NULL)
    {
        //printf("cuou\n");
        return NULL;
    }
    template_f* f=m->get_facep(m,ee->fid);

    if(f==NULL||m->vertex_is_boundary(m,*(f->vertices[0]))||m->vertex_is_boundary(m,*(f->vertices[1])))
    {
       // printf("cuowu1\n");
        return NULL;
    }


    template_v* v1=f->vertices[0],*v2=f->vertices[1],
        *n_v=m->create_vertexv(m,ee->p,3);
    LB_Matrix* m1=(LB_Matrix*)(tree_v->find(tree_v,v1->id));
    LB_Matrix* m2=(LB_Matrix*)(tree_v->find(tree_v,v2->id));
    LB_Matrix* m3=matrix_add(m1,m2);

    tree_v->erase(tree_v,v1->id);
    tree_v->erase(tree_v,v2->id);

    tree_v->insert(tree_v,n_v->id,m3);

    lb_matrix_free(m1);lb_matrix_free(m2);
    Int_RB_Tree* cells=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(cells);

    for(auto vcit=m->vc_begin(m,*v1);vcit!=m->vc_end(m,*v1);vcit++)
    {
        cells->insert(cells,quote(vcit)->id,quote(vcit));
    }
    for(auto vcit=m->vc_begin(m,*v2);vcit!=m->vc_end(m,*v2);vcit++)
    {
        cells->insert(cells,quote(vcit)->id,quote(vcit));
    }

    //Node* boundary_hfaces=NULL;
    Int_RB_Tree* tree_boundary_fs=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree_boundary_fs);

    //n_v和tree_boundary_fs可能存在折叠

    for(auto it=cells->begin(cells);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(it.second);
        for(auto chfit=m->chf_begin(m,*c);chfit!=m->chf_end(m,*c);chfit++)
        {
            template_hf* hf=quote(chfit);
            if(hf->vertices[0]!=v1&&hf->vertices[0]!=v2&&hf->vertices[1]!=v1&&hf->vertices[1]!=v2)
            {
                template_v**vs=(template_v**)(tree_boundary_fs->find(tree_boundary_fs,hf->face->id));
                if(vs==NULL )
                {
                    vs=(template_v**)malloc(sizeof(template_v*)*3);
                    vs[0]=hf->vertices[0];vs[1]=hf->vertices[1];vs[2]=n_v;
                    tree_boundary_fs->insert(tree_boundary_fs,hf->face->id,vs);
                }
                else
                {
                    free(vs);
                    tree_boundary_fs->erase(tree_boundary_fs,hf->face->id);

                }
                // Node *hfs=NULL;
                // hfs=node_overlying(hfs,hf->vertices[1]);
                // hfs=node_overlying(hfs,hf->vertices[0]);
                // boundary_hfaces=node_overlying(boundary_hfaces,hfs);
            }
        }
    }

    for(auto it=cells->begin(cells);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(it.second);
        if(!m->delete_cell(m,*c,true))
        {
            printf("cuowudelete\n");
        }
    }
    int_rb_tree_free(cells);

    for(auto it=tree_boundary_fs->begin(tree_boundary_fs);it.it!=NULL;it++)
    {
        template_v**vs=(template_v**)(it.second);

        template_c* c=m->create_cellv(m,vs,3);
        free(vs);
    }

    //free_node(boundary_hfaces);
    int_rb_tree_free(tree_boundary_fs);

    for(auto vfit=m->vf_begin(m,*n_v);vfit!=m->vf_end(m,*n_v);vfit++)
    {
        if(ch->size>=ch->max_size){return n_v;}
        double* p=compute_qem_one_edge_position(m,quote(vfit),tree_v);
        if(p==NULL){
            continue;
        }
        double energy=qem_compute_one_edge_cost(quote(vfit),tree_v,p);
        Edge_Energy* data=(Edge_Energy*)malloc(sizeof(Edge_Energy));
        edge_energy_init(data);
        data->energy=energy;
        data->p=p;
        data->fid=quote(vfit)->id;
        //printf("ch o fidb %d\n",((Edge_Energy*)(ch->values[0]))->fid);

        insert_cheap(ch,data);
        //printf("ch o fide %d\n",((Edge_Energy*)(ch->values[0]))->fid);

    }


    return n_v;
}
